WOLF core
WOLF: A Modular Estimation Framework for Robotics
XXXXXXXXXXXXXXXXXXXXXXXXvoid(0)" frameborder="0" name="MSearchResults" id="MSearchResults">
Public Member Functions | Protected Member Functions | List of all members
wolf::NodeStateBlocks Class Referenceabstract

#include <core/common/node_state_blocks.h>

Inheritance diagram for wolf::NodeStateBlocks:
[legend]

Public Member Functions

 NodeStateBlocks (const std::string &_category, const std::string &_type, const TypeComposite &_state_types, const VectorComposite &_state_vectors, const PriorComposite &_state_priors)
 Constructor. More...
 
virtual ~NodeStateBlocks ()
 
virtual YAML::Node toYaml () const
 
StateKeys getKeys () const
 
TypeComposite getStateTypes () const
 
bool has (const char &_sb_key) const
 
bool has (const std::string &_keys) const
 
virtual unsigned int id () const =0
 
std::map< char, StateBlockConstPtr > getStateBlockMap () const
 
std::map< char, StateBlockPtr > getStateBlockMap ()
 
std::vector< StateBlockConstPtr > getStateBlockVec () const
 
std::vector< StateBlockPtr > getStateBlockVec ()
 
StateBlockConstPtr getP () const
 
StateBlockConstPtr getO () const
 
StateBlockPtr getP ()
 
StateBlockPtr getO ()
 
virtual void fix ()
 
virtual void unfix ()
 
virtual bool isFixed () const
 
StateBlockPtr emplaceStateBlock (const char key, const std::string &_type, const Eigen::VectorXd &_vector, const bool &_fixed)
 Emplace a state block. More...
 
StateBlockPtr emplaceStateBlockZero (const char key, const std::string &_type, const bool &_fixed)
 Emplace a state block with zero state. More...
 
bool hasStateBlock (const StateBlockConstPtr &_sb) const
 
bool setStateBlock (const char _sb_key, const StateBlockPtr &_sb)
 
bool stateBlockKey (const StateBlockConstPtr &_sb, char &_key) const
 
virtual StateBlockConstPtr getStateBlock (const char &_sb_key) const
 
virtual StateBlockPtr getStateBlock (const char &_sb_key)
 
void setProblem (ProblemPtr _problem) override
 
virtual void remove (bool viral_remove_parent_without_children=false)
 
bool hasChildren () const override
 
void emplaceFactorStateBlock (const char &_key, const Eigen::VectorXd &_x, const Eigen::MatrixXd &_cov, unsigned int _start_idx=0)
 Add an absolute factor to a parameter. More...
 
void emplacePriors ()
 
FactorBaseConstPtrSet getFactoredBySet () const
 
FactorBasePtrSet getFactoredBySet ()
 
void getFactoredBySet (FactorBaseConstPtrSet &_fac_set) const
 
void getFactoredBySet (FactorBasePtrSet &_fac_set)
 
FactorBaseConstPtrSet getSensoryFactorSet () const
 
FactorBasePtrSet getSensoryFactorSet ()
 
std::map< char, FactorBaseConstPtr > getPriorFactorMap () const
 
std::map< char, FactorBasePtr > getPriorFactorMap ()
 
FactorBaseConstPtr getPriorFactor (char) const
 
FactorBasePtr getPriorFactor (char)
 
FactorBaseConstPtrList getFactorsOf (ProcessorBaseConstPtr _processor_ptr, const std::string &type="") const
 
FactorBasePtrList getFactorsOf (ProcessorBasePtr _processor_ptr, const std::string &type="")
 
bool hasFactor (FactorBaseConstPtr _fac) const
 
VectorComposite getState (const StateKeys &_keys="") const
 
void setState (const VectorComposite &_state, const bool _notify=true)
 
void setState (const Eigen::VectorXd &_state, const StateKeys &_keys, const std::list< SizeEigen > &_sizes, const bool _notify=true)
 
void setState (const Eigen::VectorXd &_state, const StateKeys &_keys, const bool _notify=true)
 
void setState (const StateKeys &_keys, const std::list< Eigen::VectorXd > &_vectors, const bool _notify=true)
 
Eigen::VectorXd getStateVector (const StateKeys &_keys) const
 
unsigned int getSize (const StateKeys &_keys="") const
 
unsigned int getLocalSize (const StateKeys &_keys="") const
 
bool getCovariance (const StateKeys &_keys, Eigen::MatrixXd &_cov) const
 
void perturb (double amplitude=0.01)
 perturb all states states with random noise following an uniform distribution in [ -amplitude, amplitude ] More...
 
bool isCapture () const
 
bool isFrame () const
 
bool isLandmark () const
 
bool isSensor () const
 
- Public Member Functions inherited from wolf::NodeBase
 NodeBase (const std::string &_category, const std::string &_type="Undefined", const std::string &_name="")
 
virtual ~NodeBase ()=default
 
unsigned int nodeId () const
 
std::string getCategory () const
 
std::string getType () const
 
std::string getName () const
 
bool isRemoving () const
 
virtual bool hasChildren () const =0
 
void setType (const std::string &_type)
 
void setName (const std::string &_name)
 
ProblemConstPtr getProblem () const
 
ProblemPtr getProblem ()
 

Protected Member Functions

virtual void removeStateBlock (const char &_sb_key)
 
void transform (const VectorComposite &_transformation)
 
void printState (bool _factored_by, bool _metric, bool _state_blocks, std::ostream &_stream, std::string _tabs) const
 
CheckLog localCheck (bool _verbose, std::ostream &_stream, std::string _tabs) const
 
virtual void setProblem (ProblemPtr _prob_ptr)
 

Additional Inherited Members

- Protected Attributes inherited from wolf::NodeBase
ProblemWPtr problem_ptr_
 
unsigned int node_id_
 Node id. It is unique over the whole Wolf Tree. More...
 
std::string node_category_
 Text label identifying the category of node ("SENSOR", "FEATURE", etc) More...
 
std::string node_type_
 Text label identifying the type or subcategory of node ("Pin Hole", "Point 2d", etc) More...
 
std::string node_name_
 
bool is_removing_
 A flag for safely removing nodes from the Wolf tree. See remove(). More...
 

Constructor & Destructor Documentation

◆ NodeStateBlocks()

wolf::NodeStateBlocks::NodeStateBlocks ( const std::string &  _category,
const std::string &  _type,
const TypeComposite &  _state_types,
const VectorComposite &  _state_vectors,
const PriorComposite &  _state_priors 
)

Constructor.

Parameters
_categoryTO BE HARDCODED IN THE DERIVED CLASS CONSTRUCTOR: category name (see node_base.h)
_typeTO BE HARDCODED IN THE DERIVED CLASS CONSTRUCTOR: derived type name
_state_typesComposite of types of the states that the node can have. Probably to be hardcoded in the derived class constructor.
_state_vectorsComposite of vectors of the states of the node. A 'StateBlock' for each key in '_state_vectors' will be emplaced (of the type specified in '_state_types'). Not all keys of '_state_types' are required to be specified, but all keys of '_state_vectors' should be in '_state_types'.
_state_priorsComposite of priors (initial_guess, factor or fixed) of the states of the node. Not all keys of '_state_vectors' are required to be specified, but all keys of '_state_priors' should be in '_state_vectors'. If not specified, no prior is assumed, i.e. 'initial_guess'.

◆ ~NodeStateBlocks()

wolf::NodeStateBlocks::~NodeStateBlocks ( )
inlinevirtual

Member Function Documentation

◆ emplaceFactorStateBlock()

void wolf::NodeStateBlocks::emplaceFactorStateBlock ( const char &  _key,
const Eigen::VectorXd &  _x,
const Eigen::MatrixXd &  _cov,
unsigned int  _start_idx = 0 
)

Add an absolute factor to a parameter.

Add an absolute factor to a parameter

Parameters
_istate block index (in state_block_vec_) of the parameter to be constrained
_xprior value
_covcovariance
_start_idxstate segment starting index (not used in quaternions)
_sizestate segment size (-1: whole state) (not used in quaternions)

◆ emplacePriors()

void wolf::NodeStateBlocks::emplacePriors ( )

◆ emplaceStateBlock()

StateBlockPtr wolf::NodeStateBlocks::emplaceStateBlock ( const char  key,
const std::string &  _type,
const Eigen::VectorXd &  _vector,
const bool &  _fixed 
)

Emplace a state block.

Parameters
_keyKey of the new state.
_typeTypes of the new state.
_vectorVector of the new state.
_fixedIf the new state is fixed (i.e. not estimated).

◆ emplaceStateBlockZero()

StateBlockPtr wolf::NodeStateBlocks::emplaceStateBlockZero ( const char  key,
const std::string &  _type,
const bool &  _fixed 
)

Emplace a state block with zero state.

Parameters
_keyKey of the new state.
_typeTypes of the new state.
_fixedIf the new state is fixed (i.e. not estimated).

◆ fix()

void wolf::NodeStateBlocks::fix ( )
inlinevirtual

Reimplemented in wolf::CaptureBase.

◆ getCovariance()

bool wolf::NodeStateBlocks::getCovariance ( const StateKeys &  _keys,
Eigen::MatrixXd &  _cov 
) const

◆ getFactoredBySet() [1/4]

FactorBasePtrSet wolf::NodeStateBlocks::getFactoredBySet ( )
inline

◆ getFactoredBySet() [2/4]

FactorBaseConstPtrSet wolf::NodeStateBlocks::getFactoredBySet ( ) const
inline

◆ getFactoredBySet() [3/4]

void wolf::NodeStateBlocks::getFactoredBySet ( FactorBaseConstPtrSet &  _fac_set) const
inline

◆ getFactoredBySet() [4/4]

void wolf::NodeStateBlocks::getFactoredBySet ( FactorBasePtrSet &  _fac_set)
inline

◆ getFactorsOf() [1/2]

FactorBaseConstPtrList wolf::NodeStateBlocks::getFactorsOf ( ProcessorBaseConstPtr  _processor_ptr,
const std::string &  type = "" 
) const

◆ getFactorsOf() [2/2]

FactorBasePtrList wolf::NodeStateBlocks::getFactorsOf ( ProcessorBasePtr  _processor_ptr,
const std::string &  type = "" 
)

◆ getKeys()

StateKeys wolf::NodeStateBlocks::getKeys ( ) const
inline

◆ getLocalSize()

unsigned int wolf::NodeStateBlocks::getLocalSize ( const StateKeys &  _keys = "") const

◆ getO() [1/2]

wolf::StateBlockPtr wolf::NodeStateBlocks::getO ( )
inline

◆ getO() [2/2]

wolf::StateBlockConstPtr wolf::NodeStateBlocks::getO ( ) const
inline

◆ getP() [1/2]

wolf::StateBlockPtr wolf::NodeStateBlocks::getP ( )
inline

◆ getP() [2/2]

wolf::StateBlockConstPtr wolf::NodeStateBlocks::getP ( ) const
inline

◆ getPriorFactor() [1/2]

FactorBasePtr wolf::NodeStateBlocks::getPriorFactor ( char  key)
inline

◆ getPriorFactor() [2/2]

FactorBaseConstPtr wolf::NodeStateBlocks::getPriorFactor ( char  key) const
inline

◆ getPriorFactorMap() [1/2]

std::map< char, FactorBasePtr > wolf::NodeStateBlocks::getPriorFactorMap ( )
inline

◆ getPriorFactorMap() [2/2]

std::map< char, FactorBaseConstPtr > wolf::NodeStateBlocks::getPriorFactorMap ( ) const
inline

◆ getSensoryFactorSet() [1/2]

FactorBasePtrSet wolf::NodeStateBlocks::getSensoryFactorSet ( )
inline

◆ getSensoryFactorSet() [2/2]

FactorBaseConstPtrSet wolf::NodeStateBlocks::getSensoryFactorSet ( ) const
inline

◆ getSize()

unsigned int wolf::NodeStateBlocks::getSize ( const StateKeys &  _keys = "") const

◆ getState()

VectorComposite wolf::NodeStateBlocks::getState ( const StateKeys &  _keys = "") const

◆ getStateBlock() [1/2]

wolf::StateBlockPtr wolf::NodeStateBlocks::getStateBlock ( const char &  _sb_key)
inlinevirtual

Reimplemented in wolf::CaptureBase, and wolf::SensorBase.

◆ getStateBlock() [2/2]

wolf::StateBlockConstPtr wolf::NodeStateBlocks::getStateBlock ( const char &  _sb_key) const
inlinevirtual

Reimplemented in wolf::CaptureBase, and wolf::SensorBase.

◆ getStateBlockMap() [1/2]

std::map< char, StateBlockPtr > wolf::NodeStateBlocks::getStateBlockMap ( )
inline

◆ getStateBlockMap() [2/2]

std::map< char, StateBlockConstPtr > wolf::NodeStateBlocks::getStateBlockMap ( ) const
inline

◆ getStateBlockVec() [1/2]

std::vector< StateBlockPtr > wolf::NodeStateBlocks::getStateBlockVec ( )
inline

◆ getStateBlockVec() [2/2]

std::vector< StateBlockConstPtr > wolf::NodeStateBlocks::getStateBlockVec ( ) const
inline

◆ getStateTypes()

TypeComposite wolf::NodeStateBlocks::getStateTypes ( ) const
inline

◆ getStateVector()

VectorXd wolf::NodeStateBlocks::getStateVector ( const StateKeys &  _keys) const

◆ has() [1/2]

bool wolf::NodeStateBlocks::has ( const char &  _sb_key) const
inline

◆ has() [2/2]

bool wolf::NodeStateBlocks::has ( const std::string &  _keys) const
inline

◆ hasChildren()

bool wolf::NodeStateBlocks::hasChildren ( ) const
inlineoverridevirtual

Implements wolf::NodeBase.

Reimplemented in wolf::FrameBase.

◆ hasFactor()

bool wolf::NodeStateBlocks::hasFactor ( FactorBaseConstPtr  _fac) const
inline

◆ hasStateBlock()

bool wolf::NodeStateBlocks::hasStateBlock ( const StateBlockConstPtr &  _sb) const

◆ id()

virtual unsigned int wolf::NodeStateBlocks::id ( ) const
pure virtual

Implemented in wolf::CaptureBase, wolf::FrameBase, wolf::LandmarkBase, and wolf::SensorBase.

◆ isCapture()

bool wolf::NodeStateBlocks::isCapture ( ) const
inline

◆ isFixed()

bool wolf::NodeStateBlocks::isFixed ( ) const
virtual

◆ isFrame()

bool wolf::NodeStateBlocks::isFrame ( ) const
inline

◆ isLandmark()

bool wolf::NodeStateBlocks::isLandmark ( ) const
inline

◆ isSensor()

bool wolf::NodeStateBlocks::isSensor ( ) const
inline

◆ localCheck()

CheckLog wolf::NodeStateBlocks::localCheck ( bool  _verbose,
std::ostream &  _stream,
std::string  _tabs 
) const
protected

◆ perturb()

void wolf::NodeStateBlocks::perturb ( double  amplitude = 0.01)

perturb all states states with random noise following an uniform distribution in [ -amplitude, amplitude ]

◆ printState()

void wolf::NodeStateBlocks::printState ( bool  _factored_by,
bool  _metric,
bool  _state_blocks,
std::ostream &  _stream,
std::string  _tabs 
) const
protected

◆ remove()

void wolf::NodeStateBlocks::remove ( bool  viral_remove_parent_without_children = false)
virtual

Reimplemented in wolf::CaptureBase, wolf::FrameBase, and wolf::LandmarkBase.

◆ removeStateBlock()

void wolf::NodeStateBlocks::removeStateBlock ( const char &  _sb_key)
protectedvirtual

◆ setProblem()

void wolf::NodeStateBlocks::setProblem ( ProblemPtr  _problem)
overridevirtual

Reimplemented from wolf::NodeBase.

Reimplemented in wolf::LandmarkBase, wolf::FrameBase, and wolf::SensorBase.

◆ setState() [1/4]

void wolf::NodeStateBlocks::setState ( const Eigen::VectorXd &  _state,
const StateKeys &  _keys,
const bool  _notify = true 
)

◆ setState() [2/4]

void wolf::NodeStateBlocks::setState ( const Eigen::VectorXd &  _state,
const StateKeys &  _keys,
const std::list< SizeEigen > &  _sizes,
const bool  _notify = true 
)

◆ setState() [3/4]

void wolf::NodeStateBlocks::setState ( const StateKeys &  _keys,
const std::list< Eigen::VectorXd > &  _vectors,
const bool  _notify = true 
)

◆ setState() [4/4]

void wolf::NodeStateBlocks::setState ( const VectorComposite &  _state,
const bool  _notify = true 
)

◆ setStateBlock()

bool wolf::NodeStateBlocks::setStateBlock ( const char  _sb_key,
const StateBlockPtr &  _sb 
)
inline

◆ stateBlockKey()

bool wolf::NodeStateBlocks::stateBlockKey ( const StateBlockConstPtr &  _sb,
char &  _key 
) const

◆ toYaml()

YAML::Node wolf::NodeStateBlocks::toYaml ( ) const
virtual

Reimplemented in wolf::LandmarkBase.

◆ transform()

void wolf::NodeStateBlocks::transform ( const VectorComposite &  _transformation)
protected

◆ unfix()

void wolf::NodeStateBlocks::unfix ( )
inlinevirtual

Reimplemented in wolf::CaptureBase.

The documentation for this class was generated from the following files: